/*
 * Gyro.c
 *
 * Created: 1/19/2013 7:10:41 PM
 *  Author: Alexander
 */ 

#include "Gyro.h"
#include "Vector_Class.h"

/*******************************************************************************
* Function Name		: L3GD20_ReadReg
* Description		: Generic Reading function. It must be fullfilled with either
*					: I2C or SPI reading functions
* Input				: Register Address
* Output			: Data REad
* Return			: None
*******************************************************************************/
uint8_t L3GD20_ReadReg(uint8_t Reg, uint8_t* Data)
{
	
	//To be completed with either I2c or SPI reading function
	//i.e.: *Data = SPI_Mems_Read_Reg( Reg );
	
	PIND |= (1<<PD1);
	WriteByteSPI(Reg|(1<<7));
	*Data = ReadByteSPI(Reg|(1<<7));
	PIND |= (1<<PD1);
	
	return 1;
}


/**************************************************************************
* Method:		L3GD20_Read_Vectors_All
* FullName:		L3GD20_Read_Vectors_All
* Access:		public 
* Returns:		uint8_t
* Qualifier:	
* Parameter:	AxesVector_t * Data
**************************************************************************/
uint8_t L3GD20_Read_Vectors_All(AxesVector_t* Data)
{
	uint8_t LowVal;
	uint8_t HighVal;
	
	PIND |= (1<<PD1);
	
	WriteByteSPI(L3GD20_OUT_X_L|((1<<7)|(1<<6)));
	HighVal = ReadByteSPI(1<<7);
	LowVal = ReadByteSPI(1<<7);
	Data->AXIS_X = (int16_t)((HighVal << 8 ) | LowVal);
	
	HighVal = ReadByteSPI(1<<7);
	LowVal = ReadByteSPI(1<<7);
	Data->AXIS_Y = (int16_t)((HighVal << 8 ) | LowVal);

	HighVal = ReadByteSPI(1<<7);
	LowVal = ReadByteSPI(1<<7);
	Data->AXIS_Z = (int16_t)((HighVal << 8 ) | LowVal);
	
	PIND |= (1<<PD1);
	
	return 1;
}

/*******************************************************************************
* Function Name		: L3GD20_WriteReg
* Description		: Generic Writing function. It must be fullfilled with either
*					: I2C or SPI writing function
* Input				: Register Address, Data to be written
* Output			: None
* Return			: None
*******************************************************************************/
uint8_t L3GD20_WriteReg(uint8_t Reg, uint8_t Data) {
	
	//To be completed with either I2c or SPI writing function
	//i.e.: SPI_Mems_Write_Reg(Reg, Data);
	
	PIND |= (1<<PD1);
	WriteByteSPI(Reg);
	WriteByteSPI(Data);
	PIND |= (1<<PD1);
	
	return 1;
}


/**************************************************************************
* Method:		Gyro_Init
* FullName:		Gyro_Init
* Access:		public  
* Returns:		void
* Qualifier:	
* Parameter:	void
**************************************************************************/
void Gyro_Init(void)
{
	DDRD |= (1<<PD1);
	PIND |= (1<<PD1);
	uint8_t status = 0;
	uint8_t response;
	
	/*
	 *	Output Data Rate:	12.5
	 *	BandWidth:			95 Hz
	 *	Z Axis Enable:
	 *	X Axis Enable:
	 *	Y Axis Enable:
	 */
	L3GD20_WriteReg(L3GD20_CTRL_REG1, ( (L3GD20_ODR_190Hz_BW_25 << L3GD20_ODR_BIT)| (L3GD20_NORMAL << L3GD20_PD)
										| L3GD20_Z_ENABLE | L3GD20_X_ENABLE | L3GD20_Y_ENABLE));
	
	/*
	 *	Endianess:				Most Significant Bit first
	 *	Full Scale Selection:	250 degrees per second
	 */
	L3GD20_WriteReg(L3GD20_CTRL_REG4, ( (L3GD20_BLE_MSB << L3GD20_BLE) | (L3GD20_FULLSCALE_250 << L3GD20_FS) ));
}

/**************************************************************************
* Method:		Zero_Rate_Calc
* FullName:		Zero_Rate_Calc
* Access:		public 
* Returns:		AxesVector_t
* Qualifier:	
**************************************************************************/
AxesVector_t Zero_Rate_Calc()
{
	int count = 0;
	
	AxesVector_t average;
	AxesVector_t temp;
	Vector_Init(&temp);
	Vector_Init(&average);
	
	
	for (count ; count < 100; count ++)
	{
		L3GD20_Read_Vectors_All(&temp);
		
		average.AXIS_X += temp.AXIS_X;
		average.AXIS_Y += temp.AXIS_Y;
		average.AXIS_Z += temp.AXIS_Z;
	}
	
			
	average.AXIS_X = average.AXIS_X/100;
	average.AXIS_Y = average.AXIS_Y/100;
	average.AXIS_Z = average.AXIS_Z/100;
	
	return average;
}

/**************************************************************************
* Method:		Calculate_Angle_Rate
* FullName:		Calculate_Angle_Rate
* Access:		public 
* Returns:		void
* Qualifier:	
* Parameter:	AxesVector_t * vector_raw
* Parameter:	AxesVector_t * zero_rate_vector
**************************************************************************/
void Calculate_Angle_Rate(AxesVector_t *vector_raw, AxesVector_t *zero_rate_vector)
{
	int16_t scale_factor = 875;
	int16_t raw_x = Get_X(vector_raw);
	int16_t raw_y = Get_Y(vector_raw);
	int16_t raw_z = Get_Z(vector_raw);
	
	int16_t zero_x = Get_X(zero_rate_vector);
	int16_t zero_y = Get_Y(zero_rate_vector);
	int16_t zero_z = Get_Z(zero_rate_vector);
	
	raw_x = ((raw_x - zero_x)*7)/800;
	raw_y = ((raw_y - zero_y)*7)/800;
	raw_z = ((raw_z - zero_z)*7)/800;
	Update_Values(vector_raw, raw_x, raw_y, raw_z);
}

/**************************************************************************
* Method:		Calculate_Angle
* FullName:		Calculate_Angle
* Access:		public 
* Returns:		void
* Qualifier:	
* Parameter:	AxesVector_t * zero_rate_vector
* Parameter:	AxesVector_t * vector
* Parameter:	AxesVector_t * angle
**************************************************************************/
void Calculate_Angle(AxesVector_t* zero_rate_vector, AxesVector_t* vector, AxesVector_t* angle)
{
	L3GD20_Read_Vectors_All(vector);
	
	Calculate_Angle_Rate(vector,zero_rate_vector);
	
	Add_Vectors(angle, vector);
	Increment_Count(angle);
}